/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
*/
#include <string.h>
#include "printf_inc.h"
#include "FlexCAN_Ip.h"
#include "sq_buffer/sq_buffer.h"
#include "switch0_can_gateway_ipc_server.h"
#include "switch0_canoe_ipc_client.h"
#include "hscg_firmware_statistic.h"
#include "can_output.h"
#include "mcu_id.h"
#include "route.h"
#include "eth2can.h"
#include "can_msg.h"
#include "can_config.h"
#include "gmac_time.h"
#include "vcan.h"

#define EXTRACT_CAN_CS_ATTR(_CS, _TO_STU)  \
do {                                    \
(_TO_STU)->dlc = (uint8)(((_CS)&FLEXCAN_IP_CS_DLC_MASK) >> FLEXCAN_IP_CS_DLC_SHIFT);    \
(_TO_STU)->rtr = (((_CS) & FLEXCAN_IP_CS_RTR_MASK) == 0 ? 0:1);   \
(_TO_STU)->ide = (((_CS) & FLEXCAN_IP_CS_IDE_MASK) == 0 ? 0:1);   \
(_TO_STU)->srr = (((_CS) & FLEXCAN_IP_CS_SRR_MASK) == 0 ? 0:1);   \
(_TO_STU)->esi = (((_CS) & FLEXCAN_IP_CS_ESI_MASK) == 0 ? 0:1);   \
(_TO_STU)->brs = (((_CS) & FLEXCAN_IP_CS_BRS_MASK) == 0 ? 0:1);   \
(_TO_STU)->edl = (((_CS) & FLEXCAN_IP_CS_EDL_MASK) == 0 ? 0:1);   \
} while (0)

#define IS_EXT_FRAME_MSG(_msg) ((((_msg)->cs & FLEXCAN_IP_CS_IDE_MASK) ? 1 : 0))


/**
 * @brief 调整时间戳，时间戳的来源有两个：
 *        1. GMAC的时间戳(80bit)
 *        2. Enhanced FIFO时间戳(32bit)
 *        填入的规则为：
 *            1.取Enhanced FIFO时间戳的32bit, 此时间戳在收到CAN报文时，硬件会自动填入
 *            2.读取GMAC时间戳
 *            3.比较1和2的低32bit，判断是否进位（一次进位时间约为4s，理论上不会出现进二位或者以上的情况）
*/
static void adjust_timestamp(uint32 enhanced_timestamp, uint32 *timestamp_seconds)
{
    struct timespec64 gmac_timestamp = {0};

    (void)gmac_get_phc_time(&gmac_timestamp);

    /* 如果翻转，则认为发生了进位 */
    *timestamp_seconds = (gmac_timestamp.tv_nsec < enhanced_timestamp) ? (gmac_timestamp.tv_sec - 1) : gmac_timestamp.tv_sec;

    //print gmac_timestamp_second_l and gmac_timestamp_nanosecond
    hprintf(DEBUG, "adjust_timestamp tv_sec:%d nanosecond:0x%x gmac_timestamp_second_l:%d enhanced_fifo:0x%x\n",
        gmac_timestamp.tv_sec, gmac_timestamp.tv_nsec, *timestamp_seconds, enhanced_timestamp);
}

static void fill_timestamp(CAN2X* can2x, volatile uint32 enhanced_timestamp)
{
    CAN2X_PARAMS *can2x_param = can2x->param;

    can2x_param->gmac_timestamp_second_h = 0;
    can2x_param->gmac_timestamp_nanosecond = enhanced_timestamp;
    adjust_timestamp(enhanced_timestamp, &can2x_param->gmac_timestamp_second_l);

    STATISTICS_UPDATE_PACKET_CAN2ETH_TIMESTAMP_BY_VAULE(TIMESTAMP_PACKET_CAN2ETH_HW_RECV_CAN_PACKET,
        (((u64)can2x_param->gmac_timestamp_second_l) << 32) | can2x_param->gmac_timestamp_nanosecond);
}

static void can_build_can2x_from_msg(uint8 controller_id, const Flexcan_Ip_MsgBuffType *msg, CAN2X *can2x)
{
    CAN2X_PARAMS *can2x_param = can2x->param;
    uint32 msg_ctrl = msg->cs;
    uint32 i;

    fill_timestamp(can2x, msg->time_stamp);

    EXTRACT_CAN_CS_ATTR(msg_ctrl, can2x);

    can2x->id = msg->msgId;
    can2x_param->src_can_bus_id = controller_id;
    can2x_param->filter_id = can_hit_id_to_filter_id(controller_id, IS_EXT_FRAME_MSG(msg), msg->id_hit);

    // byteorder
    for (i = 0; i < (msg->dataLen + 3U) / 4; i++) {
        uint32 *src = &(msg->data[i * 4]);
        uint32 *dst = &(can2x->data[i * 4]);
        *dst = __builtin_bswap32(*src);
    }
}

static void can_forward_to_local_ports(const LLCG_Can2CanOptions *route2can, uint8 controller_id,
                                        uint8 hwobject_id, Flexcan_Ip_MsgBuffType *msg)
{
    Flexcan_Ip_MsgBuffType *tx_msg;
    const uint8 *u8DestHwChList = route2can->u8DestHwChList;
    uint32 queue_work_bitmap;
    uint8 dest_ctrl_id;
    sint8 mb_idx;
    sint32 ret;
    uint32 i;

#ifdef STATISTICS_PACKET_CAN2CAN_TIMESTAMP_ENABLE
    {
        uint32 seconds;

        adjust_timestamp(msg->time_stamp, &seconds);
        STATISTICS_UPDATE_PACKET_CAN2CAN_TIMESTAMP_BY_VAULE(TIMESTAMP_PACKET_CAN2CAN_HW_RECV_CAN_PACKET,
            (((u64)seconds) << 32) | msg->time_stamp);
    }
#endif

    /* fast forward */
    queue_work_bitmap = 0;
    for (i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; ++i) {
        if (!u8DestHwChList[i]) {
            continue;
        }

        dest_ctrl_id = i;
        mb_idx = can_tx_mb_idx_query(controller_id, hwobject_id, dest_ctrl_id);
        hprintf(DEBUG, "dest_ctrl_idx:%d mb_idx:%d\n", dest_ctrl_id, mb_idx);
        if (mb_idx < 0) {
            queue_work_bitmap |= (1 << dest_ctrl_id);
        } else {
            STATISTICS_UPDATE_PACKET_CAN2CAN_TIMESTAMP(TIMESTAMP_PACKET_CAN2CAN_SENDTO_CAN_CONTROLLER_START);
            FlexCAN_Transmit(Flexcan_Ip_apxBase[dest_ctrl_id], mb_idx, msg->cs,
                                msg->msgId, msg->data, msg->dataLen);
            STATISTICS_UPDATE_PACKET_CAN2CAN_TIMESTAMP(TIMESTAMP_PACKET_CAN2CAN_SENDTO_CAN_CONTROLLER_END);
            STATS_INC_CAN_TX(dest_ctrl_id);
        }
    }

    if (!queue_work_bitmap) {
        return;
    }

    /* queue work forward */
    for (i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; ++i) {
        if (!(queue_work_bitmap & (1 << i))) {
            continue;
        }

        tx_msg = can_msg_shallow(msg);
        ret = can_txque_enqueue(i, tx_msg);
        if (ret != 0) {
            hprintf(ERROR, "enqueue to tx pool:%d failed:%d!\n", i, ret);
            can_msg_free(tx_msg);
            STATS_INC_CAN_TX_ERR(i);
        }
    }
}

static void can_forward_to_other_cores(uint8 controller_id,
                                        const LLCG_RouteTableBaseType *route_rule,
                                        const Flexcan_Ip_MsgBuffType *msg)
{
    SQE *sq_buffer;
    CAN2X *can2x;
    CAN2X_PARAMS *can2x_param;
    uint32 consumer_mask;

    sq_buffer = SQ_Buffer_Produce_Get(can2x_sq_buffer);
    if (!sq_buffer) {
        hprintf(ERROR, "Get sq buffer for dest:%d failed!\n", route_rule->Destination);
        if ((route_rule->Destination & LLCG_ROUTETABLE_CAN_2_ETH) != 0) {
            STATS_INC_ETH_TX_ERR();
        }
        return;
    }

    can2x = (X2CAN *)sq_buffer->buffer;
    can2x_param = (CAN2X_PARAMS *)can2x->param;

    consumer_mask = 0;
    broadcast_reg_entry_t ipc_broadcast_reg[VCAN_MAX_SUBCRIBED] = {0}; //ipc需要的目的地
    uint8 ipc_broadcast_reg_count = 0;
    //遍历每一个bit位, i就是对应的des_id, i=0代表CAN2CAN，上面已经处理过了,从1开始
    for(int i=1;i<VCAN_MAX_SUBCRIBED;i++) {
        int des_id = i;
        if((route_rule->Destination & (1U<<des_id)) != 0) {
            //获取对应的consumer mask，稍后填到sqbuffer共享内存中去
            int temp_consumer_mask = utils_get_consumer_mask_by_destination_id(des_id);
            if(temp_consumer_mask < 0)
                hprintf(DEBUG, "ERROR: cannot find consumer_mask by des_id:%d\n", des_id);
            else
                consumer_mask |= temp_consumer_mask;
            
            //是否要发送ipc通知
            if(vcan_check_subcribed(i)) {
                //要往这个目的地发msgbox
                broadcast_reg_entry_t *temp_ipc_broadcast_reg = vcan_get_subcribed_info(des_id);
                if(temp_ipc_broadcast_reg == NULL)
                    hprintf(DEBUG, "ERROR: cannot find broadcast_reg by des_id:%d\n", des_id);
                else {
                    memcpy(&(ipc_broadcast_reg[ipc_broadcast_reg_count]), temp_ipc_broadcast_reg, sizeof(broadcast_reg_entry_t));
                    ipc_broadcast_reg_count++;
                }
            }
        }
    }
    hprintf(DEBUG, "consumer_mask:0x%x ipc_broadcast_reg_count:%d\n", consumer_mask, ipc_broadcast_reg_count);

    /* CAN2ETH fix:CAN2ETH方向需要填mac地址*/
    if ((route_rule->Destination & LLCG_ROUTETABLE_CAN_2_ETH) != 0) {
        memcpy(can2x_param->CAN2ETH_DMAC, route_rule->Can2EthOptions.u8Can2EthDestMac, 6);
        STATS_INC_ETH_TX();
    }

    if (!consumer_mask) {
        return;
    }

    can_build_can2x_from_msg(controller_id, msg, can2x);

    STATISTICS_UPDATE_PACKET_CAN2ETH_TIMESTAMP(TIMESTAMP_PACKET_CAN2ETH_WR_CAN2ETH_SQBUF_START);
    SQ_Buffer_Produce_Put(can2x_sq_buffer, sq_buffer, consumer_mask);

    /* 发送ipc通知 */
    if(ipc_broadcast_reg_count != 0) {
        can_s_server->switch0_can_gateway_server.can_gateway_recv(&ipc_broadcast_reg, ipc_broadcast_reg_count);
    }
}

static void can_dispatch_msg(uint32 controller_id, Flexcan_Ip_MsgBuffType *msg)
{
    const LLCG_RouteTableBaseType *route_rule;
    uint32 filter_id;
    uint8 hwobject_id;

    STATS_INC_CAN_RX(controller_id);
    filter_id = can_hit_id_to_filter_id(controller_id, IS_EXT_FRAME_MSG(msg), msg->id_hit);
    hwobject_id = can_filter_id_to_hwobject_id(controller_id, IS_EXT_FRAME_MSG(msg), filter_id);

    route_rule = can_route_query(controller_id, hwobject_id, msg->msgId);
    if((route_rule == NULL) || (route_rule->Destination == 0)) {
        hprintf(DEBUG, "error: controller:%u filter_id:%d hwobject_id:%d no route, drop!\n",
            controller_id, msg->id_hit, hwobject_id);
        STATS_INC_CAN_RX_DROP(controller_id);
        return;
    }

    hprintf(DEBUG, "filter_id:%u hwobject_id:%d route destination:0x%x\n",
        filter_id, hwobject_id, route_rule->Destination);

    /* local port forward */
    if ((route_rule->Destination & LLCG_ROUTETABLE_CAN_2_CAN) != 0) {
        can_forward_to_local_ports(&route_rule->Can2CanOptions, controller_id, hwobject_id, msg);
    }

    /* forward to other core */
    if ((route_rule->Destination & (~LLCG_ROUTETABLE_CAN_2_CAN))) {
        can_forward_to_other_cores(controller_id, route_rule, msg);
    }
}

/**
 * @brief read can message and forward.
 */
void can_input_run(void)
{
    Flexcan_Ip_MsgBuffType *msg;
    uint32 i;

    for (i = 0; i < LLCG_CONFIG_MAXCTRL_COUNT; i++) {
        if (!can_is_forward_port(i)) {
            continue;
        }

        if (!FlexCAN_IsRxFifoDataAvailable(Flexcan_Ip_apxBase[i])) {
            continue;
        }

        msg = can_msg_alloc();
        if (!msg) {
            hprintf(ERROR, "error, alloc msg failed!\r\n");
            continue;
        }

        FlexCAN_ReadEnhancedRxFifo(Flexcan_Ip_apxBase[i], msg);

        can_dispatch_msg(i, msg);

        can_msg_free(msg);
    }
}
